#include <string.h>
#include <video/VGA.h>

void *memset(void *dest, _s32 val, size_t n)
{
  char *tmp = (_8 *) dest;
  for( ; n != 0; n--) *tmp++ = val;

  return dest;
}

_u16 *memsetw(_u16 *dest, _u16 val, size_t n)
{
  _u16 *tmp = (_u16 *)dest;
  for( ; n != 0; n--) *tmp++ = val;

  return dest;
}

void *memcpy(void *dest, const void *src, size_t n)
{
  const _8 *sp = (const _8 *)src;
  _8 *dp = (_8 *)dest;
  for(; n != 0; n--) *dp++ = *sp++;

  return dest;
}

size_t strlen(const _8 *str)
{
  size_t retval;
  for(retval = 0; *str != '\0'; str++) retval++;

  return retval;
}

static void printc(_8 **str, _s32 c)
{
  if (str) 
  {
    **str = c;
    ++(*str);
  }
  else
    putc(c);
}

#define PAD_RIGHT 1
#define PAD_ZERO 2
static int prints(_8 **out, const _8 *string, _s32 width, _s32 pad)
{
  register _s32 pc = 0, padchar = ' ';
  if (width > 0) 
  {
    register _s32 len = 0;
    register const _8 *ptr;

    for (ptr = string; *ptr; ++ptr) ++len;

    if (len >= width) width = 0;

    else width -= len;

    if (pad & PAD_ZERO) padchar = '0';
  }

  if (!(pad & PAD_RIGHT)) 
  {
    for ( ; width > 0; --width) 
    {
      printc(out, padchar);
      ++pc;
    }
  }

  for ( ; *string ; ++string) 
  {
    printc(out, *string);
    ++pc;
  }

  for ( ; width > 0; --width) 
  {
    printc(out, padchar);
    ++pc;
  }

  return pc;
}

#define PRINT_BUF_LEN 12
static int printi(_8 **out, _s32 i, _s32 b, _s32 sg, _s32 width, _s32 pad, _s32 letbase)
{
  _8 print_buf[PRINT_BUF_LEN];
  register _8 *s;
  register _s32 t, neg = 0, pc = 0;
  register _u32 u = i;
  if (i == 0) 
  {
    print_buf[0] = '0';
    print_buf[1] = '\0';

    return prints (out, print_buf, width, pad);
  }

  if (sg && b == 10 && i < 0) 
    {
      neg = 1;
      u = -i;
    }

  s = print_buf + PRINT_BUF_LEN-1;
  *s = '\0';

  while (u) 
  {
    t = u % b;
    if( t >= 10 )
    t += letbase - '0' - 10;
    *--s = t + '0';
    u /= b;
  }

  if (neg) 
  {
    if( width && (pad & PAD_ZERO) ) 
    {
      printc(out, '-');
      ++pc;
      --width;
    }

    else
    {
      *--s = '-';
    }
  }

  return pc + prints(out, s, width, pad);
}

static int print(_8 **out, _s32 *varg)
{
  register _s32 width, pad;
  register _s32 pc = 0;
  register _8 *format = (_8 *)(*varg++);
  char scr[2];

  for (; *format != 0; ++format) 
  {
    if (*format == '%') 
    {
      ++format;
      width = pad = 0;
      if (*format == '\0') break;
      if (*format == '%') goto out;

      if (*format == '-') 
      {
        ++format;
        pad = PAD_RIGHT;
      }

      while (*format == '0') 
      {
        ++format;
        pad |= PAD_ZERO;
      }

      for ( ; *format >= '0' && *format <= '9'; ++format) 
      {
        width *= 10;
        width += *format - '0';
      }

      if( *format == 's' ) 
      {
        register char *s = *((char **)varg++);
        pc += prints(out, s?s:"(null)", width, pad);

        continue;
      }

      if( *format == 'd' ) 
      {
        pc += printi(out, *varg++, 10, 1, width, pad, 'a');
        continue;
      }

      if( *format == 'x' ) 
      {
        pc += printi(out, *varg++, 16, 0, width, pad, 'a');
        continue;
      }

      if( *format == 'X' ) 
      {
        pc += printi(out, *varg++, 16, 0, width, pad, 'A');
        continue;
      }

      if( *format == 'u' ) 
      {
        pc += printi(out, *varg++, 10, 0, width, pad, 'a');
        continue;
      }

      if( *format == 'c' ) 
      {
        /* char are converted to int then pushed on the stack */
        scr[0] = *varg++;
        scr[1] = '\0';
        pc += prints(out, scr, width, pad);
        continue;
      }
    }

    else
    {
      out:
      printc(out, *format);
      ++pc;
    }
  }

  if (out) **out = '\0';

  return pc;
}

/* assuming sizeof(void *) == sizeof(int) */
int printk(const char *format, ...)
{
  register int *varg = (int *)(&format);
  return print(0, varg);
}

int sprintk(char *out, const char *format, ...)
{
  register int *varg = (int *)(&format);
  return print(&out, varg);
}
